#ifndef _B_FORM_
#define _B_FORM_
#include "Function.h"
#include "spline.h"
#include <vector>
#include <eigen3/Eigen/Sparse>
#include <eigen3/Eigen/Dense>
#include <iostream>
using namespace std;

class Bspline_base : public Function
{
private:
    int degree;
    vector<double> node;

public:
    Bspline_base() = default; //For robust
    Bspline_base(int _degree, vector<double> _node): degree(_degree), node(_node)
    {
        if (degree != node.size() - 2)
	    {
		    cerr<< "error:The k does not match." <<endl;
		    exit(-1);
	    }
    } 
    ~Bspline_base()
	{
	    node.clear();
	}
    
    double operator()(double _x)
    {
        
        if (degree == 1)  
        {
            if (_x >node[0] && _x <node[1])
                return (_x -node[0])/( node[1] -node[0]);
            else if (_x >=node[1] && _x <node[2])
                return (node[2] - _x)/(node[2] -node[1]);
            else
                return 0;
        }
        else
        {
        //Lemma 3.23
        vector<double>node1(node.begin(),node.end() - 1);
        vector<double>node2(node.begin() + 1,node.end());

        Bspline_base B1(degree - 1,node1);
        Bspline_base B2(degree - 1,node2);

        return (_x -node[0])*B1(_x)/(node[degree] -node[0]) + (node[degree+1] - _x)*B2(_x)/(node[degree+1] -node[1]);
        }
    }
};


class Bspline : public spline
{
private:
    vector<double> P, P1, P2;
    vector<Bspline_base> B;
    Function &f;
    int n, choice, k; 
    Eigen::VectorXd co; 
public:
    Bspline() = default;

    Bspline(Function &_f, vector<double> _P, int _choice, int _k): f(_f), P(_P), choice(_choice), k(_k)
    {
        if ((choice != 1) && (choice != 2) && (choice != 3))
	    {
		    cerr<< "error:Invalid choice ." <<endl;
		    exit(-1);
	    }
        if ((k != 1) && (k != 2) && (k != 3))
	    {
		    cerr<< "error:Invalid k." <<endl;
		    exit(-1);
	    }

        n = P.size();

        if (k == 1)
        {
            P1 = {P[0]-1*(P[1]-P[0])};
            P2 = {P[n-1]+1*(P[1]-P[0])};
        }
        if (k == 3)
        {
            P1 = {P[0]-3*(P[1]-P[0]), P[0]-2*(P[1]-P[0]), P[0]-1*(P[1]-P[0])};
            P2 = {P[n-1]+1*(P[1]-P[0]), P[n-1]+2*(P[1]-P[0]), P[n-1]+3*(P[1]-P[0])};
        }

        P.insert(P.begin(), P1.begin(), P1.end());
        P.insert(P.end(), P2.begin(), P2.end());

        for (int i = 2-k; i < n + 1; i++)
            {
                vector<double> v(P.begin()+i-2+k, P.begin()+i+2*k);
                Bspline_base b(k, v);
                B.push_back(b); // B存储需要用的所有B样条基函数
            }
    }

    void cal()
    {
        if (k == 3)
        {
            Eigen::SparseMatrix<double> A(n+2, n+2);
            vector<Eigen::Triplet<double> > tripL;
            Eigen::MatrixXd y(n+2,1); 
            y = Eigen::MatrixXd::Zero(n+2, 1); 

            for (int i = 1; i < n + 1; i++)
		    {
                tripL.push_back(Eigen::Triplet<double>(i, i-1, B[i-1](P[i+2])));
                tripL.push_back(Eigen::Triplet<double>(i, i, B[i](P[i+2])));
                tripL.push_back(Eigen::Triplet<double>(i, i+1, B[i+1](P[i+2])));
                
                y(i, 0) = f(P[i+2]);
		    }

            if (choice ==  1)
            {
                vector<double> v0(P.begin()+1, P.begin()+5);
                vector<double> v1(P.begin()+2, P.begin()+6);
                vector<double> vn_1(P.end()-6, P.end()-2);
                vector<double> vn(P.end()-5, P.end()-1);
                
                Bspline_base b0(2, v0), b1(2, v1), bn_1(2, vn_1), bn(2, vn);

                tripL.push_back(Eigen::Triplet<double>(0, 0, (-3.0*b0(P[3]))/(P[4]-P[1])));
                tripL.push_back(Eigen::Triplet<double>(0, 1, (3.0*b0(P[3]))/(P[4]-P[1]) - (3.0*b1(P[3]))/(P[5]-P[2])));
                tripL.push_back(Eigen::Triplet<double>(0, 2, (3.0*b1(P[3]))/(P[5]-P[2])));
                tripL.push_back(Eigen::Triplet<double>(n+1, n-1, (-3.0*bn_1(P[n+2]))/(P[n+3]-P[n])));
                tripL.push_back(Eigen::Triplet<double>(n+1, n, (3.0*bn_1(P[n+2]))/(P[n+3]-P[n]) - (3.0*bn(P[n+2]))/(P[n+4]-P[n+1])));
                tripL.push_back(Eigen::Triplet<double>(n+1, n+1, (3.0*bn(P[n+2]))/(P[n+4]-P[n+1])));

                y(0, 0) = f.diff(P[3]);
                y(n+1, 0) = f.diff(P[n+2]);
            }
            else
            {
                tripL.push_back(Eigen::Triplet<double>(0, 0, 6.0/((P[4]-P[1])*(P[4]-P[2]))));
                tripL.push_back(Eigen::Triplet<double>(0, 1, -6.0/((P[4]-P[1])*(P[4]-P[2])) - 6.0/((P[5]-P[2])*(P[4]-P[2]))));
                tripL.push_back(Eigen::Triplet<double>(0, 2, 6.0/((P[5]-P[2])*(P[4]-P[2]))));
                tripL.push_back(Eigen::Triplet<double>(n+1, n-1, 6.0/((P[n+3]-P[n])*(P[n+3]-P[n+1]))));
                tripL.push_back(Eigen::Triplet<double>(n+1, n, -6.0/((P[n+3]-P[n])*(P[n+3]-P[n+1])) - 6.0/((P[n+4]-P[n+1])*(P[n+3]-P[n+1]))));
                tripL.push_back(Eigen::Triplet<double>(n+1, n+1, 6.0/((P[n+4]-P[n+1])*(P[n+3]-P[n+1]))));
                
                if (choice ==  2)
                {
                    y(0, 0) = f.diff2(P[3]);
                    y(n+1, 0) = f.diff2(P[n+2]);
                }
                if (choice ==  3)
                {
                    y(0, 0) = 0.0;
                    y(n+1, 0) = 0.0;
                }
            }

            A.setFromTriplets(tripL.begin(), tripL.end());
            A.makeCompressed();

            Eigen::SparseLU<Eigen::SparseMatrix<double> > cal_co;

            cal_co.compute(A);
            co = cal_co.solve(y);
        }
        if (k == 1)
        {
            Eigen::SparseMatrix<double> A(n, n);
            vector<Eigen::Triplet<double> > tripL;
            Eigen::MatrixXd y(n, 1); 
            y = Eigen::MatrixXd::Zero(n, 1); // 解方程Ax = y

            for (int i = 0; i < n; i++)
		    {
                tripL.push_back(Eigen::Triplet<double>(i, i, 1.0));
                y(i, 0) = f(P[i+1]);
		    }

            A.setFromTriplets(tripL.begin(), tripL.end());
            A.makeCompressed();

            Eigen::SparseLU<Eigen::SparseMatrix<double> > cal_co;
            cal_co.compute(A);
            co = cal_co.solve(y);
        }
    }



    // 返回插值函数在_x处的函数值。
    double operator()(double _x)
    {
        if (k == 3)
        {
            if ((_x < P[3]) || (_x > P[n+2]))
	        {
		        cerr<< "error: Out of range." <<endl;
		        exit(-1);
	        }
            if (_x == P[3])
                return f(P[3]);

            int Position = 1;
            while (_x > P[Position+3])
            {
                Position = Position + 1;
            }
            
            return co(Position-1)*B[Position-1](_x) + co(Position)*B[Position](_x) + co(Position+1)*B[Position+1](_x) + co(Position+2)*B[Position+2](_x);    
        }
        if (k == 1)
        {
            if ((_x < P[1]) || (_x > P[n]))
	        {
		        cerr<< "error: Out of range." <<endl;
		        exit(-1);
	        }
            if (_x == P[1])
                return f(P[1]);

            int Position = 1;
            while (_x > P[Position+1])
            {
                Position = Position + 1;
            }

            return co(Position-1)*B[Position-1](_x) + co(Position)*B[Position](_x);      
        }
        return 0;    
    }


};

#endif
